#include "bernsteinpolyline.h"
#include <qmath.h>
#include "matrix.h"

int fat(int n)
{
    if(n == 0)
        return 1;

    int v = n;
    for(int i = 2; i < n; ++i)
        v *= i;

    return v;
}

int comb(int n, int i)
{
    return (double)(fat(n))/(fat(i)*fat(n-i));
}

BernsteinPolyLine::BernsteinPolyLine(QObject *parent) :
    QObject(parent), m(1,1)
{
}

bool BernsteinPolyLine::bindControlPoints(const Curve3D& c)
{
    Matrix contrains(c.size(),c.size());
    int n = c.size() -1;

    for(int i = 0; i <= n; ++i)
    {
        for(int j = 0; j <= i; ++j)
        {
            int l = i-j;
            int c = n-i;

            int sig = j%2==0 ? 1 : -1;

            contrains(l,c) = comb(n,i)*comb(i,j)*sig;
        }
    }


    Matrix points(c.size(),3);
    for(int i =0; i < c.size(); ++i)
    {
        points(i,0) = c[i][0];
        points(i,1) = c[i][1];
        points(i,2) = c[i][2];
    }

    m = contrains*points;

    return true;
}

Curve3D BernsteinPolyLine::getPolyLine(float step)
{
    Curve3D ret;
    for(float t = 0.0; t < 1.00001; t += step)
    {
        Matrix tt(1,m.nRows());
        tt(0,m.nRows()-1) = 1.0;
        for(int i = m.nRows()-2; i >= 0; --i)
            tt(0,i) = tt(0,i+1)*t;

        tt = tt*m;

        Point3D v;
        v[0] = tt(0,0);
        v[1] = tt(0,1);
        v[2] = tt(0,2);
        ret.push_back(v);
    }
    return ret;
}

Point3D BernsteinPolyLine::getPoint(float t)
{
    Matrix tt(1,m.nRows());
    tt(0,m.nRows()-1) = 1.0;
    for(int i = m.nRows()-2; i >= 0; --i)
        tt(0,i) = tt(0,i+1)*t;

    tt = tt*m;

    Point3D p;
    p[0] = tt(0,0);
    p[1] = tt(0,1);
    p[2] = tt(0,2);
    return p;
}


template<unsigned int ndim>
QDebug operator<<(QDebug dbg, const PointN<float, ndim> &p)
{
    dbg.nospace() << ndim << "D(";
    for(int j = 0; j < ndim-1; ++j)
        dbg.nospace() << p[j] << ",";

    dbg.nospace() << p[ndim-1] << ")";
    return dbg.space();
}

QDebug operator<<(QDebug dbg, const Curve3D &curve)
{
    dbg.nospace() << "Curve3D( ";
    for(int j = 0; j < curve.size()-1; ++j)
        dbg.nospace() << curve[j] << ", ";

    dbg.nospace() << curve[curve.size()-1] << " )";
    return dbg.space();
}
